#ifndef RRT_IMP_H
#define RRT_IMP_H


#include "Collision_detector.h"


class Rrt_Imp
{
public:

	Rrt_Imp (Collision_detector* col):
	  m_collision(col)
	{}

	/*list<Point> imp(list<Point>	route)
	{
		bool imp_done=false;
		vector<Point> vec(route.begin(), route.end());
		list <Point> imp_route;	
		if (vec.size() == 0) 
		{
			return list<Point>(); 
		}
		Point target = vec[vec.size()-1];
		for(int i=0 ; i<vec.size();i++)
		{
			for(int j=vec.size()-1 ; j>=i;j--)
			{
				Point point_new = m_collision->steer(vec[i],vec[j]);
				
				if(point_new == vec[j])
				{
					imp_done=true;
					vec[i+1]=vec[j];
					vec.erase(vec.begin()+i+2,vec.begin()+j+1);
					i=j;
					break;
				}
				if(vec[i] == target)
				{
					break;
				}
			}
		}
		if(imp_done)
		{
			imp_route=route;
		}
		else
		{
			//std::copy (vec.begin (), vec.end (), std::back_inserter (imp_route));
		}
		return imp_route;
	}*/

private:
	Collision_detector*		m_collision;
};


#endif
